#include "outputdata.h"
#include "usart.h"
#include "bsp_pid.h"

float OutData[4] = { 0 };



void OutData_test(float pitch, float roll, float yaw)
{
//    OutData[0]=(pitch+100)*100;//发送加速度初步计算的角度
//    OutData[1]=(roll+100)*100;//发送陀螺仪角速度
    OutData[0]=(roll+100)*100;//发送陀螺仪角速度
    OutData[1]=(yaw+100)*100;//发送数据融合得到的角度
//    OutData[2]=(yaw+100)*100;//发送数据融合得到的角度
}


/**
  * @brief  设置波形数据
  * @param  Value1：波形1的数值。
  * @param  Value2：波形2的数值。
  * @param  Value3：波形3的数值。
  * @param  Value4：波形4的数值。
  * @retval None
  */
void SET_OutData(float Value1, float Value2, float Value3, float Value4)
{
  OutData[0] = Value1;
  OutData[1] = Value2;
  OutData[2] = Value3;
  OutData[3] = Value4;
}

/**
  * @brief  基于CRC-16算法的循环冗余校验函数。
  * @param  Buf：指向存储数据的缓冲区的指针。
  * @param  CRC_CNT：要计算CRC的字节数。
  * @retval CRC_Temp：CRC校验结果。
  */
unsigned short CRC_CHECK(unsigned char *Buf, unsigned char CRC_CNT)
{
    unsigned short CRC_Temp;
    unsigned char i,j;
    CRC_Temp = 0xffff;

    for (i=0;i<CRC_CNT; i++){      
        CRC_Temp ^= Buf[i];
        for (j=0;j<8;j++) {
            if (CRC_Temp & 0x01)
                CRC_Temp = (CRC_Temp >>1 ) ^ 0xa001;
            else
                CRC_Temp = CRC_Temp >> 1;
        }
    }
    return(CRC_Temp);
}

/**
  * @brief  使用串口将波形数据发送给上位机。
  * @param  None
  * @retval None
  */
void OutPut_Data(void)
{
  int temp[4] = {0};
  unsigned int temp1[4] = {0};
  unsigned char databuf[10] = {0};
  unsigned char i;
  unsigned short CRC16 = 0;
  for(i=0;i<4;i++)
   {
    
    temp[i]  = (int)OutData[i];
    temp1[i] = (unsigned int)temp[i];
    
   }
   
  for(i=0;i<4;i++) 
  {
    databuf[i*2]   = (unsigned char)(temp1[i]%256);
    databuf[i*2+1] = (unsigned char)(temp1[i]/256);
  }
  
  CRC16 = CRC_CHECK(databuf,8);
  databuf[8] = CRC16%256;
  databuf[9] = CRC16/256;
  
  for(i=0;i<10;i++)
    //uart_putchar(databuf[i]);
		HAL_UART_Transmit(&huart3,&databuf[i],1,0xFF);
}
